// file iostream
#include <fstream>
#include <iostream>
// ros
#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32MultiArray.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <string>
#include <sstream>
#include <sys/stat.h>
#include <signal.h>
#include <stdio.h>
#include <condition_variable>
#include <gtsam/nonlinear/ISAM2.h>
#include <Eigen/Geometry>

// dcl_slam define
#include "distributedMapping.h"
#include "paramsServer.h"
#include "dcl_slam/loop_info.h"
#include "dcl_slam/global_descriptor.h"
#include "dcl_slam/neighbor_estimate.h"

// pcl滤波头文件
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

// log
#include <glog/logging.h>

#include "ndt_match.h"

using namespace gtsam;
using namespace std;

/* ******************初始化参数定义****************** */
std::mutex mtx;                     // 进程锁
std::condition_variable sig_buffer; // 终止信号
bool flg_stop = false;              // 程序退出标志位
bool time_count = false;            // 每隔10秒输出一个标志位

// 停止更新矩阵
static bool is_C2A_optimal = false; // 新增标志位，用于判断是否已经找到最佳分数的C2A变换矩阵
static bool is_B2A_optimal = false;

// 定义变换矩阵的保存位置
#define DIR_OUTPUT std::getenv("HOME") + std::string("/out/dcl_output")
std::string trans_B2A = DIR_OUTPUT + "/trans_B2A.csv";
std::string trans_C2A = DIR_OUTPUT + "/trans_C2A.csv";
std::string trans_C2B = DIR_OUTPUT + "/trans_C2B.csv";
// 创建并初始化文件
std::ofstream file_B2A(trans_B2A);
std::ofstream file_C2A(trans_C2A);
std::ofstream file_C2B(trans_C2B);

Eigen::Matrix4f result_B2A = Eigen::Matrix4f::Identity();
Eigen::Matrix4f result_C2A = Eigen::Matrix4f::Identity();
Eigen::Matrix4f result_C2B = Eigen::Matrix4f::Identity();

// 程序终止进程
void Stop_flg(int sig)
{
    // ROS_WARN("process stop!");
    flg_stop = true;
}

void Callback_ABC(const sensor_msgs::PointCloud2ConstPtr &pointMsgXYZI_A,
                  const sensor_msgs::PointCloud2ConstPtr &pointMsgXYZI_B,
                  const sensor_msgs::PointCloud2ConstPtr &pointMsgXYZI_C)
{
    ROS_INFO("ABC数据接收");
    // ROS_INFO("接收到数据");
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp_a(new pcl::PointCloud<pcl::PointXYZI>());
    if (pointMsgXYZI_A->width != 0)
        pcl::fromROSMsg(*pointMsgXYZI_A, *cloud_temp_a);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp_b(new pcl::PointCloud<pcl::PointXYZI>());
    if (pointMsgXYZI_B->width != 0)
        pcl::fromROSMsg(*pointMsgXYZI_B, *cloud_temp_b);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp_c(new pcl::PointCloud<pcl::PointXYZI>());
    if (pointMsgXYZI_C->width != 0)
        pcl::fromROSMsg(*pointMsgXYZI_C, *cloud_temp_c);

    // 对于点云进行滤波处理
    pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;
    voxel_grid_filter.setLeafSize(0.2f, 0.2f, 0.2f);
    voxel_grid_filter.setInputCloud(cloud_temp_a);
    voxel_grid_filter.filter(*cloud_temp_a);
    voxel_grid_filter.setInputCloud(cloud_temp_b);
    voxel_grid_filter.filter(*cloud_temp_b);
    voxel_grid_filter.setInputCloud(cloud_temp_c);
    voxel_grid_filter.filter(*cloud_temp_c);

    if (cloud_temp_a->empty() && cloud_temp_b->empty() && cloud_temp_c->empty())
    {
        ROS_ERROR("点云为空");
        return;
    }

    if (!is_B2A_optimal)
    {
        // 从b到a的变换矩阵
        sent_path(1, trans_B2A);
        result_B2A = NDTMatching_M(cloud_temp_b, cloud_temp_a, result_B2A,1);
        // result_B2A_pre = result_B2A;
        float score_B2A = getScore(1);
        if (score_B2A < 0.05)
        {
            clearFile(trans_B2A);
            ROS_INFO("最终B2A score: %f", score_B2A);
            writeMatrixToCSV(trans_B2A, result_B2A);
            is_B2A_optimal = true; // 设置标志位，表示已经找到最佳分数
        }
        // else if (score_B2A < 0.5 && score_B2A < previous_score_B2A)
        // {
        //     clearFile(trans_B2A);
        //     ROS_INFO("写入B2A score: %f", score_B2A);
        //     writeMatrixToCSV(trans_B2A, result_B2A);
        //     previous_score_B2A = score_B2A;
        // }
    }
    else
    {
        ROS_INFO("已找到最佳B2A score，不再写入");
    }

    if (!is_C2A_optimal)
    {
        sent_path(2, trans_C2A);
        // 从c到a的变换矩阵
        result_C2A = NDTMatching_M(cloud_temp_c, cloud_temp_a, result_C2A,2);
        float score_C2A = getScore(2);

        if (score_C2A < 0.05)
        {
            clearFile(trans_C2A);
            ROS_INFO("最佳C2A score: %f", score_C2A);
            writeMatrixToCSV(trans_C2A, result_C2A);
            is_C2A_optimal = true; // 设置标志位，表示已经找到最佳分数
        }
        // else if (score_C2A < 0.5 && score_C2A < previous_score_C2A)
        // {
        //     clearFile(trans_C2A);
        //     ROS_INFO("写入C2A score: %f", score_C2A);
        //     writeMatrixToCSV(trans_C2A, result_C2A);
        //     previous_score_C2A = score_C2A;
        // }
        // else
        // {
        //     result_C2A = init_guess_Get(2);
        //     // ROS_INFO("舍弃C2A");
        // }
    }
    else
    {
        ROS_INFO("已找到最佳C2A score，不再写入");
    }
}

/* void Callback_AB(const sensor_msgs::PointCloud2ConstPtr &pointMsgXYZI_A,
                 const sensor_msgs::PointCloud2ConstPtr &pointMsgXYZI_B)
{
    ROS_INFO("AB数据接收");
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp_a(new pcl::PointCloud<pcl::PointXYZI>());
    if (pointMsgXYZI_A->width != 0)
        pcl::fromROSMsg(*pointMsgXYZI_A, *cloud_temp_a);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp_b(new pcl::PointCloud<pcl::PointXYZI>());
    if (pointMsgXYZI_B->width != 0)
        pcl::fromROSMsg(*pointMsgXYZI_B, *cloud_temp_b);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp_c(new pcl::PointCloud<pcl::PointXYZI>());

    pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;
    voxel_grid_filter.setLeafSize(0.1, 0.1, 0.1);
    voxel_grid_filter.setInputCloud(cloud_temp_b);
    voxel_grid_filter.filter(*cloud_temp_b);
    if (cloud_temp_a->empty() && cloud_temp_b->empty())
    {
        ROS_INFO("点云为空");
        return;
    }
    if (!is_B2A_optimal)
    {
        // 从b到a的变换矩阵
        sent_path(1,trans_B2A);
        result_B2A = init_guess_Get(1);
        result_B2A = NDTMatching_M(cloud_temp_b, cloud_temp_a, result_B2A);
        float score_B2A = getScore(1);
        if (score_B2A < 0.05)
        {
            clearFile(trans_B2A);
            ROS_INFO("最终B2A score: %f", score_B2A);
            writeMatrixToCSV(trans_B2A, result_B2A);
            is_B2A_optimal = true; // 设置标志位，表示已经找到最佳分数
        }
    }
    else
    {
        ROS_INFO("已找到最佳B2A score，不再写入");
    }
}

void Callback_AC(const sensor_msgs::PointCloud2ConstPtr &pointMsgXYZI_A,
                 const sensor_msgs::PointCloud2ConstPtr &pointMsgXYZI_C)
{
    ROS_INFO("AC数据接收");
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp_a(new pcl::PointCloud<pcl::PointXYZI>());
    if (pointMsgXYZI_A->width != 0)
        pcl::fromROSMsg(*pointMsgXYZI_A, *cloud_temp_a);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp_c(new pcl::PointCloud<pcl::PointXYZI>());
    if (pointMsgXYZI_C->width != 0)
        pcl::fromROSMsg(*pointMsgXYZI_C, *cloud_temp_c);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_temp_b(new pcl::PointCloud<pcl::PointXYZI>());

    pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter;
    voxel_grid_filter.setLeafSize(0.1, 0.1, 0.1);
    voxel_grid_filter.setInputCloud(cloud_temp_c);
    voxel_grid_filter.filter(*cloud_temp_c);
    if (cloud_temp_a->empty() && cloud_temp_c->empty())
    {
        ROS_INFO("点云为空");
        return;
    }
    if (!is_C2A_optimal)
    {
        // 从c到a的变换矩阵
        result_C2A = init_guess_Get(2);
        result_C2A = NDTMatching_M(cloud_temp_c, cloud_temp_a, result_C2A);
        float score_C2A = getScore(2);

        if (score_C2A < 0.05)
        {
            clearFile(trans_C2A);
            ROS_INFO("最佳C2A score: %f", score_C2A);
            writeMatrixToCSV(trans_C2A, result_C2A);
            is_C2A_optimal = true; // 设置标志位，表示已经找到最佳分数
        }
    }
    else
    {
        ROS_INFO("已找到最佳C2A score，不再写入");
    }
}
 */
int main(int argc, char **argv)
{
    ros::init(argc, argv, "gt_map_node");
    ros::NodeHandle nh;
    setlocale(LC_ALL, "");
    // 判断文件夹是否存在
    if (!directoryExists(DIR_OUTPUT))
    {
        if (!createDirectory(DIR_OUTPUT))
        {
            return -1;
        }
    }
    // 判断文件是否存在
    if (!createFile(trans_B2A) || !createFile(trans_C2A))
    {
        return -1;
    }
    // 调用writeMatrixToCSV
    ROS_INFO("写入初始矩阵");
    result_B2A = init_guess_Get(1);
    result_C2A = init_guess_Get(2);
    result_C2B = init_guess_Get(3);
    // 输出初始矩阵
    // std::cout << "B2A:\n"
    //           << result_B2A << std::endl;
    // std::cout << "C2A:\n"
    //           << result_C2A << std::endl;
    // std::cout << "C2B:\n"
    //           << result_C2B << std::endl;
    writeMatrixToCSV(trans_B2A, result_B2A);
    writeMatrixToCSV(trans_C2A, result_C2A);
    writeMatrixToCSV(trans_C2B, result_C2B);

    // 三辆车的情况
    ROS_INFO("plan_abc");
    message_filters::Subscriber<sensor_msgs::PointCloud2> subPointCloud_A(nh, "a/points", 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> subPointCloud_B(nh, "b/points", 1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> subPointCloud_C(nh, "c/points", 1);
    // 使用ApproximateTime
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2,
                                                            sensor_msgs::PointCloud2>
        syncPolicy;
    typedef message_filters::Synchronizer<syncPolicy> Sync;

    message_filters::Synchronizer<syncPolicy> sync(syncPolicy(10), subPointCloud_A, subPointCloud_B, subPointCloud_C);
    sync.setMaxIntervalDuration(ros::Duration(10));
    sync.registerCallback(boost::bind(&Callback_ABC, _1, _2, _3));

    // 两辆车的情况，不能与三辆车同时运行
    /*         ROS_INFO("plan_ab");
            // 接收点云计算
            message_filters::Subscriber<sensor_msgs::PointCloud2> subPointCloud_A(nh, "a/points", 1);
            message_filters::Subscriber<sensor_msgs::PointCloud2> subPointCloud_B(nh, "b/points", 1);
            // 使用ApproximateTime
            typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2>
                syncPolicy;
            typedef message_filters::Synchronizer<syncPolicy> Sync;

            message_filters::Synchronizer<syncPolicy> sync(syncPolicy(10), subPointCloud_A, subPointCloud_B);
            sync.setMaxIntervalDuration(ros::Duration(5));
            sync.registerCallback(boost::bind(&Callback_AB, _1, _2)); */

    /*     // AB和AC分开匹配
        ROS_INFO("plan_ab&ac");
        message_filters::Subscriber<sensor_msgs::PointCloud2> subPointCloud_A(nh, "a/points", 1);
        message_filters::Subscriber<sensor_msgs::PointCloud2> subPointCloud_B(nh, "b/points", 1);
        message_filters::Subscriber<sensor_msgs::PointCloud2> subPointCloud_C(nh, "c/points", 1);
        // 定义AB和AC的同步策略
        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> syncPolicyAB;
        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> syncPolicyAC;

        // 创建AB和AC的同步器
        typedef message_filters::Synchronizer<syncPolicyAB> SyncAB;
        typedef message_filters::Synchronizer<syncPolicyAC> SyncAC;

        SyncAB sync_ab(syncPolicyAB(10), subPointCloud_A, subPointCloud_B);
        SyncAC sync_ac(syncPolicyAC(10), subPointCloud_A, subPointCloud_C);

        // 设置最大时间间隔
        sync_ab.setMaxIntervalDuration(ros::Duration(10));
        sync_ac.setMaxIntervalDuration(ros::Duration(10));

        // 分别为AB和AC注册回调函数
        sync_ab.registerCallback(boost::bind(&Callback_AB, _1, _2)); // AB匹配回调
        sync_ac.registerCallback(boost::bind(&Callback_AC, _1, _2)); // AC匹配回调 */

    // 读取键盘输入的退出指令，调用函数使得标志位flg_exit为true，结束循环
    signal(SIGINT, Stop_flg);
    ros::Rate rate(20);
    while (!flg_stop && ros::ok())
    {
        ros::spinOnce();
    }

    // 正常退出程序
    return 0;
}